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ROBOTIC  TECHNOLOGY  APPUED  TO  ARMY  MOBILITY  SYSTEMS 


I.  INTRODUCTION 


Recognizing  that  the  U.S.  Amy  Corps  of  Engineers  has  a  need  for  applying  industrial 
robotic  technology  to  high-risk  and  labor  intensive  tasks  performed  in  the  environment  of  a 
modern  battlefield,  USAMERADCOM  started  a  small  investigative  project  in  a  specific  robotic 
application  area— robotics  applied  to  rapid  excavation.  The  excavator  used  was  a  modified  J. 
I.  Case  Model  35  Backhoe  mounted  on  a  highly  mobile  Mercedes  Unimog  truck. 

The  conditions  of  the  modem  battlefield  make  rapid  excavation  extremely  hazardous  to  the 
human  operators  due  to  the  advent  of  highly  accurate,  technologically  sophisticated  weapons 
systems  and  the  possibility  of  nuclear,  biological,  and/or  chemical  contamination  of  the  bat¬ 
tlefield  environment. 


The  goal,  then,  of  the  project  was  to  eliminate  or  reduce  through  automation  as  much  of  the 
manual  digging  operation  of  the  backhoe  as  would  be  consistent  with  current  operating  pro¬ 
cedures/  In  the  process  of  doing  this,  the  technological  expertise  developed  in  the  area  of 
microprocessors,  sensors,  and  servo  systems  would  be  applied  to  a  broad  spectrum  of  MERAD-/ 
COM  mission  areas. 


J/U  1J> 


With  this  goal  in  mind,  what  will  be  presented  in  this  report  will  be  the  preliminary  results 
associated  with  controlling  the  backhoe  arm  through  a  series  of  sequential  motions.  The  com¬ 
plete  cycle — controlling  program  will  be  discussed  only  from  a  conceptual  viewpoint  while  the 
more  primitive  state  driving  and  state  sensing  programs  are  explained  in  detail. 

II.  HIERARCHICAL  CONTROL  SYSTEM 

The  microprocessor  control  system  consists  of  three  separate  central  processing  units  (CPUs) 
of  the  6502  type.  There  are  two  levels  of  control  associated  with  the  system.  The  first,  or 
primitive  level,  consists  of  those  programs  which  determine  the  slate  or  current  position  of  the 
backhoe  arm  (bucket,  dipper,  boom,  and  swing)  and  of  the  calculations  and  diagnostics 
associated  with  producing  the  necessary  voltages  that  drive  the  proportionally  controlled 
electro-hydraulic  servo  valves.  (See  Figure  1.) 

The  higher  level  consists  of  a  single  overall  system  control  program  which  interacts  with  the 
human  operator  in  a  real  time  basis  using  a  “menu”  driven  approach.  From  the  menu,  one  of 
three  operational  modes  can  be  chosen.  The  backhoe  can  be  entirely  controlled  by  the  operator 
using  joysticks,  or  the  operator  can  choose  a  hole  predefined  in  the  control  program,  or  the 
operator  can  input  the  dimensions  of  a  hole  to  be  excavated.  These  operation  modes  can  be 
found  in  the  program  as  subroutines  or,  as  called  for  in  PASCAL,1  procedures.  The 
microprocessors  selected  were  two  Rockwell  AIM  65s  and  one  Apple  II  system.  This  equipment 
was  used  due  to  availability  and  budgetary  constraints. 

*  The  higher  level  control  program  i"  written  in  IXSD  PASCAL  and  utilize*  on-line  floppy  dirk,  that  "tori'  the  regmrnleil  proredure,. 
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Figure  1.  Control  hierarchy. 


III.  SYSTEM  CONTROL  PROGRAM 

The  system  control  program  is  the  software  which  will  operate  the  excavator  while  running 
in  the  Apple  II  and  interfaced  by  an  RS  232  serial  connection  to  the  subordinate  AIM  65  pro¬ 
cessors  which  will  maintain  the  servo  loops. 

The  high-level  functions  necessary  to  operate  in  the  hole  template  mode  have  been  run  using 
a  graphic  kinematic  simulation  to  observe  results  and  have  operated  successfully  in  that  en¬ 
vironment. 

The  total  controller  (to  include  the  template  mode)  is  approximately  90  percent  complete  in 
detailed  code  at  this  time;  two  parts  which  require  some  additional  code  generation  before 
testing  are  the  bucket  loading  cycle  procedure  and  the  controller*to-subordinate  processor 
driver.  This  program  uses  PASCAL  as  its  implementation  language  except  for  a  few  assembly 
language  I/O  drivers  (Analog-to-digital  converter  and  serial  interface  drivers).  The  initial  draft 
of  detailed  code  is  expected  to  be  ready  for  evolutionary  testing  in  the  fall  of  1983,  after 
checkout  of  the  four  servo  loop  subsystems  under  control  of  AIM  65.1  and  AIM  65.2  is  com¬ 
pleted.  (See  Appendix  B:  Program  Control.) 
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IV.  STATE  DETERMINATION 


In  the  state  sensing  portion  of  the  state  determination  program,  the  state  vector  is  uniquely 
defined  by  the  four  angles  a ,  j3,  a,  and  <£.  These  angles  are  determined  by  the  digital  (10-bit) 
output  of  the  optical  encoders  mounted  on  the  bucket,  dipper,  boom,  and  swing  assembly  of 
the  backhoe.  (See  Figure  2.) 


Anale  Encoder# 

P  1 

2 

ft  3 

tr  * 


Figure  2.  State  determination. 


The  state  determination  program  developed  uses  the  6502  CPU  Assembly  Language  instruc¬ 
tion  set  to  control  both  the  data  bus  (AM26LS31  -  AM26LS32)  and  the  encoder  select  enable 
lines.2  (See  Appendix  A.) 

Basicallv,  the  state  determination  program  manages  or  strobes  the  optical  encoder’s  10-bit 
word  onto  the  data  bus,  such  that  only  one  10-bit  word  is  on  the  bus  line  per  decoded  encoder 
enable.  This  sequence  prevents  any  type  of  uncertainty  on  the  data  bus  from  occurring.  It  is.  of 
course,  apparent  that  the  RS422  tri-state  buses  (uses  AM26LS31  —  AM26LS32)  is  enabled  or  in 
the  transmit  mode  continuously.  By  continuously  enabling  this  parallel  bus.  each  encoder  is 
enabled  on  to  the  bus  for  transmission  of  data  to  take  place.  The  main  advantage  is  one  of  us¬ 
ing  less  hardware;  namelv.  one  RS422  parallel  bus  services  as  many  devices  as  are  strobed. 


V  IblHimt1.  **6502  language  Programming."  Mr<»raw-Hill.  pp.  25-30.  I <180.  , 
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The  enabling  sequence  is  program  controlled  by  the  state  determination  program3  using  8-bit 
words  written  onto  the  output  registers  of  the  AIM  65s’  Versatile  Interface  Adapter  (VIA).  The 
6522  VIA  is  an  enhanced  version  of  the  6502  Peripheral  Interface  Adapter  (PIA).4  The  main 
difference  between  the  VIA  and  the  PIA  is  that  the  VIA  provides  all  the  functions  of  the  PIA 
plus  2-bit  counter/timers  and  an  8-bit  shift  register.  The  shift  register  is  a  particularly  useful 
addition  since  it  allows  for  fast  transfer  of  serial  data  between  the  two  AIM  65s  without  a 
special  bus  interface.  This  area  will  be  described  in  more  detail  in  the  next  section. 

There  are  only  2  bits  used  of  the  8-bit  word  sent  to  the  output  registers  of  the  VIA  for  the  en¬ 
coder  enable.  The  actual  optical  encoder  is  generated  by  a  demultiplexed/decoded  high  output 
generated  for  a  SN74156/SN7404  encoder  select  circuit. 

The  AIM  65  applications  connector  which  ties  in  directly  to  the  applications  VIA,  provides 
pins  PB2  for  the  coded  signal  that  is  used  by  the  demultiplexer/decoder  to  output  lows  to  the 
HEX  inverters.  As  previously  stated,  PB3  and  PB2  are  2  bits  of  an  8-bit  word  sent  under  pro¬ 
gram  control  to  the  VIA.  The  actual  assignment  of  the  control  words  bits  as  either  input  or  out¬ 
put  ports  of  the  VIA  is  done  utilizing  the  memory  mapped  registers  provided  to  the  AIM’s  Cen¬ 
tral  Processing  Unit  (CPU)  by  the  applications  VIA.  See  the  state  determination  program  for 
the  actual  HEX  address. 

Once  the  10-bit  optical  encoder  is  selected  into  the  VIA  pins  PA0-PA7  and  PA0-PB2,  then  a 
transfer  of  their  values  is  done  using  the  accumulation  command  LDA  from  the  HEX  address 
that  has  the  memory  mapped  I/O  port.  At  this  point,  trivial  calculations  can  be  made  to  deter¬ 
mine  that  state  vector  or  the  state  of  the  backhoe.  The  complete  flow  chart  showing  the  state 
determination  program  as  well  as  both  a  basic  driven  encoder  select  program  and  an  assembly 
language  encoder  select  program  is  shown  at  Figure  3. 
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Manual  “R6500  Microcomputer  System  Hardware,"  Rockwell  International,  pp,  5-14-6-5,  1980. 


K  *  •  300 
/«3 

0300  A9  LDA  100 
0302  80  STA  A003 
0300  A9  LDA  *FC 
0307  80  STA  A002 
030A  AO  LDA  *04 
03 OC  80  STA  AOOO 
030F  AD  LOA  A001 
0312  85  STA  30 
0214  AO  LOA  AOOO 
0317  18  CLC 
0318  38  SEC 
0319  E9  SBC  *04 
0318  85  STA  31 
0310  A9  LDA  *00 
031F  80  STA  AOOO 
0322  AO  LOA  A001 
0325  85  STA  32 
0327  AO  LOA  AOOO 
032A  18  CLC 
0328  38  SEC 
03 2C  E9  SBC  *08 


032E  85  STA  33 
0330  A9  LDA  *10 
0331  80  STA  AOOO 
0335  AD  LDA  A001 
0338  85  STA  34 
03 3 A  AO  LOA  AOOO 
0330  18  CLC 
033E  38  SEC 
033F  E9  SBC  *10 
0341  85  STA  35 
0343  A9  LDA  *30 
0345  80  STA  AOOO 
0348  AD  LDA  A001 
0328  85  STA  *36 
0340  AO  LDA  AOOO 
0350  18  CLC 
0351  38  SEC 
0352  E9  SBC  *30 
0354  85  STA  37 
0358  00  BRK 
0357  00  BRK 
0360  00  8RK 


Figure  3.  Assembly  language  ancoder  aaloct  program  and  flowchart. 


Once  the  state  vector  calculations  are  made.5  if  required,  then  the  Matrix  Transfer  program 
is  used.  Specifically,  what  is  denoted  by  Matrix  is  the  unique  set  of  four  10-hit  words  which 
define  a  point  in  configuration  space  as  a  location  point  for  the  final  bucket  position.  The 
transfer  portion  is  initiated  upon  either  a  request  from  the  Apple  microprocessor  or  the  AIM 
65.2  microprocessor.  The  requests  from  the  Apple  microprocessor  arc  done  under  separate  pro¬ 
gram  control  and  arc  periodic  system  status  updates.  The  actual  request  is  received  from  the 
Apple  bv  the  AIM  65.1  utilizing  a  RS232C  port  controlled  hv  two  6850  an  Asynchronous  ('.um* 
muiiications  Interface  Adapters  (ACIA).6  The  request  from  the  AIM  65.2  is  received  and 
enabled  utilizing  the  shift  registers  of  VI  As  and  the  control  lines  CB1.  2  and  PB4.  5. 


The  RS232C  Protocol7  used  is  essentially  one  of  first  determining  the  Data  Terminal  (DTR) 
device  and  the  Data  Set  (DSR)  device.  In  this  application,  the  DSR  device  is  the  Apple 
microprocessor  and  the  DTR  device,  the  AIM  65  microprocessor.  The  driving  rationale  for  this 
decision  is  one  of  minimizing  the  hardware  modifications  to  either  microprocessor. 


Once  the  DTR  ami  DSR  have  been  selected,  the  standard  sequence  for  communication  is 
initiated.  (See  Figure  4.) 

J.  K.  Vlakrrlx.  "Mirroronipnlrr  Arrhitrrture  ami  Programming.*'  U  ilry.  pp.  251-252.  1081. 

*  Manual  “Minlrl  7710  (California  (Computer  Sy**I«*ni>.  pp.  4-2-1-11,  1081. 

Manual  “Data  (Com  mu  Mirations  I nterf art-4051."  Tektronix.  Inr..  pp.  2- 1-2-0.  1081. 


ill 


Am-  65.1 


out 

AIM-65.1 


OTR 

AIM-65. 1 


The  communication  rate  has  been  selected  as  9600  baud  with  8-hit  words,  two  stop  bits,  one 
start  bit,  and  no  parity  check.  The  framing  and  bit  width  are  all  being  taken  care  of  by  the  on¬ 
board  ACIAs. 


The  configuration  of  the  start,  data,  and  stop  bits  are  shown  in  Figure  4.  The  bit  width 
calculation  is  based  upon  11  bits  transmitted  per  frame;  e.g.. 

Frame  Width  Calculation 


CPS  =  Characters  per  second 
9600  baud  =  Y  (CPS)  X  11  bits 


Y(CPS)  =  9600/11 

Frame  Width  =  11/Y(CPS)  =  .00114  s. 

The  basic  operation  of  the  RS232C  data  link  is  controlled  through  the  programmable  ACIAs 
on  both  the  AIM  65  interface  board  and  the  Apple  RS232C  interface  board.  The  registers  of 
the  ACIA  on  the  AIM  65  have  been  inserted  in  memory  locations  HEX  1000  through  HEX 
1008.  The  HEX  address  1000  is  the  assigned  base  address  and- all  the  internal  registers  are  ac¬ 
cessed  relative  to  this  base  address.  The  software  developed  to  initiate  the  AIM  65  ACIA 
module  first  executes  a  reset  operation,  then  writes  the  baud  rate,  word  length,  and  the  number 
of  stop  and  start  bits  into  the  R6551  ACIA  control  register.  Also,  the  internal  clock  rate  is 
selected  by  loading  a  logic  one  into  bit  four  of  the  ACIA  control  register. 


A  simple  driver  program  is  shown  in  Figure  5.  Note  that  ASCII  HEX  34  is  sent  to  the 
transmit  register  on  the  ACIA  via  the  STA  1000  command  after  first  loading  the  value  im¬ 
mediately  into  the  accumulator  using  the  LDA  #  34  command. 


*  *  •  *00 
/12 

0400  A9  LDA  #00 
0402  BO  STA  1001 
0405  80  STA  1000 
0408  A9  LDA  fU 
04 OA  80  STA  1003 
0400  A9  LDA  #0$ 
040F  80  STA  100£ 
0*12  A9  L0A  #34 
0414  80  STA  1000 
0417  40  JH#  0412 
04 IA  00  BRK 
0411  00  BRK 


Figure  5.  ACIA  driver  program. 
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At  this  point,  the  data  are  sent  to  the  Apple's  AC1A  and  received  in  register  HEX  GOAD  for 
later  use  by  the  Main  Matrix  Program  in  the  Apple  microprocessor. 


Now  as  to  the  data  transfer  between  the  state  sensing  AIM  65.1  and  the  state  driving  AIM 
65.2,  the  difference  in  data  transfer  between  the  AIMs  and  the  Apple  is  due  to  the  use  of  the 
shift  register  on  the  AIM  65s.  The  angle  change  or  relative  position  is  sent  out  in  a  96-bit  data 
stream  consisting  of  four  blocks  of  24  bits  of  information.  Each  block  contains  the  10-bit  word 
that  is  a  one-to-one  correspondence  with  the  absolute  position  and  another  8  bits  used  for  ad¬ 
dressing  and  sign.  The  serial  driver  is  shown  using  one  memory  location  in  Figure  6.  Note  that 
the  8-bit  value  is  loaded  from  location  :0400  to  the  shift  register  and  is  outputted  on  CB2  with 
CB1  containing  the  timing  information  of  the  shift  pulses.  There  is  also  a  dummy  load  to  loca¬ 
tion  :AOOA  for  initialization  of  the  shift  register. 


k  *  ■  one 

ns 

OFDC  A9  LOA  #00 
0F0E  80  STA  A006 
OFEI  A9  LOA  #14 
0FE3  80  STA  A008 
0FE6  A9  LOA  #20 
0FE8  80  STA  A008 
0FEB  A9  LOA  #00 
0FE0  80  STA  A009 
0FF0  A0  LOA  0401 
OFFS  80  STA  A00A 
OFFS  EA  NOP 
0FF7  EA  NOP 
OFFS  EA  NOP 
0FF9  00  6RK 
0FFA  00  BRIL 


Figure  6.  Serial  driver  program. 


With  the  completion  of  the  data  transfer  between  the  two  AIM  65s.  the  feedback  loop  is  com¬ 
pleted.  which  is  necessary  for  the  system  program  to  operate. 

At  this  point,  it  becomes  necessary  to  consider  specifically  the  behavior  of  the  serial  driver 
and  serial  receiver  programs  under  multiple  96-bit  data  transfer.  The  multiple  data  transfer  is 
initiated  by  the  addition  of  a  multiple  status  flag  check  using  the  X  and  Y  registers.  The 
LDX  #  00  and  LDY  #  00  commands  each  load  a  HEX  value  of  zero  into  these  registers  which 
are  subsequently  incremented  within  the  actual  data  transmission  portion  of  the  code  and  the 
multiple  transfer  portion  of  the  code.  It  had  become  apparent  after  several  data  transmission 
efforts,  that  the  inner  loop  or  the  delay  time  between  each  8-bit  word  needed  to  be  controlled 
severely  such  that  data  transmission  would  occur  only  after  a  handshake  had  been  initiated 
and  acknowledged  after  each  8-bit  word  rather  than  12  words  or  96  bits.  With  this  software 
control  in  place,  data  drop  outs  were  decreased  but  not  totally  eliminated.  At  present,  the  data 
loss  is  being  handled  by  utilizing  an  error  checking  technique.  The  error  checking  technique 
consists  of  first  determining  if  the  encoder  address  is  in  correct  sequence  and  then,  once  this 
check  is  passed  proceeding  to  evaluate  the  magnitude  of  the  current  state  change  with  the 
proceeding  one  in  order  to  determine  if  the  data  is  reasonable. 

Transmission  errors  are  a  problem  with  medium-speed  devices  in  which  several  methods 
have  been  developed  to  lessen  the  likelihood  of  such  errors;  error  checking  is  one  such  method. 
Several  others  are: 

a.  Sampling  the  input  data  at  the  center  of  the  transmission  interval  in  order  to  avoid  edge 
effects;  that  is,  keeping  away  from  the  edges  where  that  data  is  unstable. 

b.  Generating  a  checking  parity;  the  use  of  an  extra  bit  to  force  the  correct  data  even  or  odd. 

c.  Longitudinal  and  Cyclic  Redundancy  checks. 

At  present,  considerable  effort  is  being  made  to  minimize  the  transmission  errors  and  provide 
a  relatively  stable  control  signal  to  drive  the  system. 


V.  STATE  DRIVING8 

Since  the  AIM  65.2  microcomputer  controller  of  the  electro-hydraulic  valves  has  no  position 
sensors  connected  directly  to  it,  position  data  must  be  received  from  AIM  65.1  so  that  ap¬ 
propriate  voltages  can  be  sent  to  the  power  digital  to  analog  converters  which,  in  turn,  control 
the  electro-hydraulic  valves.  This  AIM  65.1/65.2  interface  consists  of  a  four-line  serial  data 
port  that  is  used  to  transmit  serial  data  between  each  of  the  AIM  65s’  8-bit  shift  registers, 
where  two  lines  from  AIM  65.2  (out  of  four  available)  are  hardwired  to  the  corresponding  two 
lines  on  the  AIM  65.1.  One  line  pair  is  used  to  transfer  the  control  clock  pulses  while  the  other 
pair  is  used  to  transfer  the  serial  data  (encoder  address  and  associated  angular  difference 
data).  The  handshake  control  of  the  data  transfer  is  done  using  two  lines  (PB6  and  PB7  on 
AIM  65.2  to  PB4  and  PB5  on  AIM  65.1,  respectively).  The  serial  data  consists  of  a  96-bit  data 
stream  with  the  encoder  address  sent  in  one  8-bit  field  and  the  associated  angular  difference 
data  sent  in  two  8-bit  fields  (contiguous  to  the  encoder  address). 

Note  that  each  8-bit  field,  as  it  is  received,  is  stored  in  consecutive  memory  locations  so  that 
the  AIM  65  control  program  can,  when  data  is  needed,  sequentially  pick  out  of  memory  the  en¬ 
coder  address  and  angular  difference  data. 

After  receiving  the  serial  data,  the  control  program  written  in  the  BASIC  programming 
language  resident  on  the  AIM  65,  then  “PEEKS”  to  the  memory  locations  holding  the  1st  en¬ 
coder  address  (eventually,  the  2nd,  3rd,  and  4th)  and  associated  angular  difference  data  and 
converts  these  binary  values  to  decimal  equivalent.  The  results  are  decoded  and  checked  for  er¬ 
rors.  If  no  errors  are  found,  the  program  proceeds  to  find  the  power  digital-to-analog  converter 
(PD AC)  output  voltage  versus  angular  difference  equation  associated  with  each  link.*  From 
this  equation,  the  PDAC  voltage  is  found  which  corresponds  to  that  angular  difference  condi¬ 
tion.  This  value  is  then  converted  to  a  decimal  number  based  on  the  resolution  of  the  PDAC 
(11  effective  bits  at  ±24-V  power  transistor  supply  voltage  or  24/(2n  —  1)  =  11.7  mV/bit 
change)  and  with  the  encoder/link  address,  outputted  to  the  parallel  output  port  of  the  AIM 
65.2.  This  output  port  actually  consists  of  two  8-bit  ports,  Port  A  (PA)  and  Port  B  (PB)  as  well 
as  the  four  serial  communications  lines.  The  eight  least  significant  bits  of  the  voltage  value  are 
outputted  on  Port  A,  while  the  remaining  three  most  significant  bits  of  the  voltage  are  out¬ 
putted  to  Port  B  along  with  the  sign  bit  and  the  2  bits  used  for  the  encoder/link  address.  (The 
remaining  2  bits  on  Port  B  are  used  for  handshake  control  of  the  serial  data  transfer.) 

Following  the  output  of  address  and  data  (including  sign  bit)  to  Ports  A  and  B,  is  a  strobe  of 
several  milliseconds  duration  that  “enables”  the  address  decoder  which,  in  turn,  sends  a  strobe 
to  the  addressed  PDAC. 


An  empirical  equation  found  by  measuring  the  response  of  each  link  to  an  applied  voltage. 
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Now,  if  errors  are  found  in  the  decoder  address  and  data,  the  program  would  shutdown  the 
backhoe,  print  a  message  stating  the  reason  for  shutdown,  and  query  the  user  as  to  what  to  do 
next.  The  program  has  two  responses  to  the  user’s  reply:  terminate  program  or  begin  again  by 
requesting  more  data  from  AIM  65.1. 

This  process  is  continued  until  the  angular  differences,  given  for  each  link,  approach  a 
tolerance  value10  close  to  0,  corresponding  to  0  V  on  the  output  voltage-angular  difference 
equation.  If  a  link  does  overshoot  the  final  position,  the  angula.  difference  data  received  from 
AIM  65.1  will  be  decoded  as  a  negative  value  which  corresponds  to  a  negative  PDAC  output 
voltage.  Consequently,  the  direction  of  link  motion  will  be  reversed  towards  the  correct  final 
position.  A  block  diagram  of  the  output  port  of  the  AIM  65.2  and  ail  that  is  connected  to  it  are 
shown  in  Figures  7  and  8,  respectively. 

The  power  digital-to-analog  converter  consists  of  a  12-bit  storage  register  with  strobed  inputs, 
a  12-bit  digital-to-analog  converter,  and  a  power  output  stage  (see  Figure  9).  The  maximum 
drive  voltage  the  servo  valves  will  receive  from  the  PDACs  is  15  V  at  100  percent  connect 
time.11  The  resolution  of  the  PDACs  given  a  supply  voltage  of  24  V  to  the  power  stage  and  an 
effective  word  length  of  11  bits,  is  11.7  mV.  Maximum  drive  current  that  the  PDAC  can  con¬ 
tinuously  supply  to  a  load  (in  this  case,  a  27-ohm  solenoid)  is  ±  1.2  A. 

The  electro-hydraulic  valve  that  replaced  the  case  valve  on  the  case  backhoe  was  the  Monsu- 
Tison  HV03  proportionally  controlled  valve  with  an  attached  Monsun-Tison  EHC3  electro- 
hydraulic  converter;  optional  joysticks  were  added.  With  these  valves,  a  nearly  linear  output 
hydraulic  pressure  input  vs.  drive  current  can  be  attained. 


An  empirical  value  bawd  on  the  response  of  each  link.  « 

A  higher  voltage  can  he  used  (up  to  24  V)  if  the  voltage  source  i*  pulsed  (."SO  [terccnt  duty  cycle)  I  his  reduce-  the 

mechanical  hysteresis  that  is  developed  in  the  solenoids  of  the  Also.  %%ith  the  higher  voltage,  an  increase  in  backhoe  arm  “poucr"  , 

can  !m*  realized. 
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Figure  7.  AIM  86.2  output  port. 


VI.  HOLE  STATE  DETERMINATION 


During  the  initial  stages  of  work  with  the  automated  hackhoe,  the  overall  system  controller 
will  be  “blind”  as  to  the  hole  state.  The  hackhoe  will  he  digging  in  soft  earth  so  the  controller 
will,  at  this  point,  assume  that  the  bucket  is  removing  a  cubic  matrix  square  with  each  bucket- 
full  of  earth  and,  when  finished  with  the  complete  digging  cycle,  assume  the  desired  hole  was 
dug  to  the  specifications  inputted  by  the  user.  But,  after  successful  demonstration  of  the  com¬ 
plete  system  controller,  a  sensorfs)  will  be  added  to  supply  this  information  to  the  controller.  A 
sensor  that  was  determined  could  supply  this  information  (hole  state  and  bucket  digging  effi¬ 
ciency)  is  a  rangefinder. 

Using  a  rangefinder,  the  controller  would  be  capable  of  making  a  hole  survey  (if  rangefinder 
was  properly  mounted)  by  comparing  the  three-dimensional  hole  matrix  that  it  generated  in  its 
control  program  to  the  results  of  the  rangefinder’s  scan.  Also,  the  controller  could  determine 
how  much  material  the  bucket  was  removing  (bucket  digging  efficiency)  by  curling  the  bucket 
to  a  predetermined  position,  then  using  a  rangefinder  (again,  properly  mounted)  to  indicate  the 
level  of  material  in  the  bucket  by  examining  the  distance  between  the  sensor  and  the  material 
in  the  bucket. 

In  evaluating  the  test  results  conducted  on  a  specific  rangefinder  (Ultrasonic  Ranging  type), 
several  problem  areas  occurred.  The  most  significant  of  these  was  the  20-degree  beam  width  of 
the  ranger.  This  beam  width  would  result  in  a  detectable  cone  diameter  of  4.21  ft  at  a  distance 
of  12  ft.  It  is  clear  that  this  large  beam  width  would  result  in  too  little  resolution  making  a  hole 
survey.  Therefore,  the  Ultrasonic  Ranging  System  tested  was  not  used  during  the  control  cycle 
tests. 


VII.  RESPONSE  OF  SYSTEM 

The  results  presented  in  this  report  are  preliminary  since  the  full  Control  System  Modeling  is 
expected  to  be  presented  in  another  report  in  the  near  future. 

Using  basic  principles,  most  dynamic  systems  are  subject  to  analysis  using  a  force  law  for¬ 
mulation  of  the  equations  of  motion.  The  force  law  can  be  stated  simply  as: 


m.r.  =  F.  '  + 


dr 


Z  iv 


where:  F^'1  represents  external  forces  to  the  ith  particle,  and 


F^  represents  internal  forces  with  F..  =  0. 


If  all  the  forces  are  summed  over  all  particles  then 


E  -r =  E  F'r'  +'ES^o I  i*  j,  a. 


i  dt 


-4  E  =  E  <? 


dt*  i 


Rewriting  with  center  of  mass  considerations, 


ZM^  ZM,r, 


MR  =  ZMjfj, 


MR=  E  Fi(6)* 


Therefore,  M 


i!  R  -2  Fi(e)  =  F(e). 
1  1 


Using  a  force  law  formulation,  what  is  needed  is  an  accurate  mathematical  representation  of 
the  system  we  intend  to  control.  Clearly,  there  are  several  major  elements;  these  are  shown  in 
Figure  10. 
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Essentially,  the  control  flow  is  as  follows.  The  reference  or  desired  position  signal  is  entered 
into  the  microprocessor  controller  section.  This  section  consists  of  the  three  6502  CPUs,  two 
AIM  65s,  and  one  Apple  microprocessor,  with  the  necessary  intercommunication  hardware 
established  as  a  separate  interface  within  this  section.  The  controller  section  produces  the  error 
signal  or  the  difference,  in  terms  of  angles,  between  the  actual  position  of  the  backhoe  arm  vs. 
the  final  desired  position.  As  stated  previously,  intercommunication  between  the  AIM  65 
microprocessors  is  based  on  a  96-bit  data  stream  of  which  10  bits  represent  the  error,  and  12 
bits  are  used  to  drive  the  electro-hydraulic  servo  valves.  This  error  signal  (12-bit)  is  decoded  by 
each  selected  high-power  digital-to-analog  converter  (PDAC)  and  the  resulting  analog  voltage 
at  the  output  of  the  selected  PDAC  is  placed  across  the  input  of  the  electro-hydraulic  servos. 
The  hydraulic  flow  utilized  by  the  servos  then  drives  the  main  cylinder  flow  which  is  used  to 
actuate  the  mechanical  links  of  the  backhoe.  At  this  point,  the  machine  is  being  driven  to  its 
final  programmed  position  with  disturbances  being  placed  at  this  section  of  the  Control 
System. 

The  return  loop  closure  is  provided  by  four  optical  encoders  mounted  at  each  link  joint  such 
that  the  final  state  of  the  arm  can  be  calculated  easily. 

Now  that  the  Control  System  block  diagram  has  been  discussed,  a  preliminary  quantitative 
analysis  as  to  the  stability  of  the  total  System  will  be  discussed  with  the  emphasis  of  analysis 
being  placed  in  the  electro-hydraulic  valve  section. 

The  input  or  desired  position  signal  is  entered  manually  using  the  AIM  65.1  ASCII 
Keyboard,  into  eight  consecutive  memory  locations.  This  information  is  used  to  calculate  the 
difference  between  the  final  state  and  the  initial  state.  The  time  delay  for  this  multiprecision 
subtraction,  as  well  as  configuration  of  the  data  stream  to  include  enabling  the  optical  en¬ 
coders  sequentially,  is  approximately  700  Hz  or  0.001  s.  Currently,  the  actual  program  has  an 
additional  0.030-s  delay  that  is  being  introduced  as  a  short  term  solution  to  the  data  loss  dur¬ 
ing  transmission  of  the  calculated  difference  angles.  Therefore,  the  error  signal  generated  and 
sent  to  the  input  of  each  of  the  PDACs  is  subject  to  a  time  delay  of  approximately  0.060  s  total 
(also  includes  the  state  driving  portion  of  the  delay  loop).  We  can  now  replace  the  first  two 
blocks  of  Figure  10  as  shown  in  Figure  11. 

Now  the  PDACs  are  Burr-Brown  Model  4804,  12-bit  power,  digital-to-analog  converters. 
Using  the  specifications  provided  by  Burr-Brown,  the  maximum  settling  time  for  any  input 
change  is  stated  as  0.001  s  with  a  maximum  output  resistance  of  1  ohm.  The  next  block  in 
Figure  10  can  be  replaced  by  essentially  a  single  time  delay  of  0.001  s  as  shown  in  Figure  12. 
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Figure  12.  Timing  t2. 


At  this  point,  we  have  now  reduced  all  the  functional  blocks  prior  to  the  electro-hydraulic 
servo  valves  to  a  series  of  time  delays  that  act  upon  the  control  signal.  The  next  step  is  to  model 
the  response  of  the  electro-hydraulic  valves  which  is  clearly  a  third  order  system.  Since  the 
complete  analysis  for  the  electro-hydraulic  valve  has  been  treated  in  detail  by  many  authors,12 
only  the  results  of  the  transient  response  characteristics  will  be  presented  here. 

The  essential  operation  of  the  electro-hydraulic  servo  valves  is  as  follows.  If  mechanical  mo¬ 
tion  is  desired  in  one  direction  or  the  other,  a  current  is  applied  to  the  windings  of  a  coil  in 
which  the  armature  is  rigidly  attached  to  the  spool  of  the  valve.  This  spool  acts  to  cover  or  un¬ 
cover  (depending  on  the  direction)  one  of  three  ports  of  the  pilot  hydraulic  pressure  loop.  When 
the  appropriate  port  is  uncovered,  hydraulic  fluid  is  conducted  at  5  gal/min  from  the  supply  to 
one  of  the  chambers  of  the  ram  cylinder,  thus,  moving  the  mechanical  links.  (See  Figure  13.) 
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Figure  13.  Electro-hydraulic  valve. 


Utilizing  an  equivalent  circuit  representation  of  this  physical  system  and  operating  the  coils 
in  a  push-pull  fashion,  the  difference  in  current  relationship  results  in  the  following  transfer 
function: 


Ej(s)  s  +  (rpDAC+RL^ 


where:  n  =  amplification  factor, 

L  =  inductance  of  the  coil, 

IH  =  differential  current, 

Ej  =  applied  current, 

Rl  =  resistance  of  windings,  and 
rpr>A<;  =  resistance  of  PDACs. 
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With  this  push-pull  operation,  the  force  developed  on  the  spool  is  defined  in  relation  to  the 
differential  current  dl: 


F  =  Mr 


where:  F  =  net  force  on  spool. 

Km  =  magnetic  force  constant,  and 

I,  =  differential  current. 

*1 

This  force  developed  on  the  spool  will  result  in  motion  of  the  spool  according  to  the  follow¬ 
ing  equation: 

F  =  M  X,  +  Kx„  +  Fr 

dt2  dt 

where:  M  =  mass  of  the  spool  armature  assembly. 
j8  =  equivalent  viscous  friction. 

K  =  restoring  spring  coefficient,  and 
Fr  =  friction  force  on  spring. 

Deriving  the  transfer  function  relating  spool  displacement  to  applied  force  results  in: 

X,(s)  _  1/M _ 

F(s)  s*  +  (B/M)s  +  K/M 


Combining  the  physical  parameters  flow,  ram  velocity  and  wetted  area  of  the  piston  face 
results  in: 


Xr(t)  =  EK 


a2-2{W„o+W  2 
^  n  n 


g-ort  +  ae^wntsin(W0t+^) 

(1  -?2)w(o2-2?aWn+Wn2)v4 


-tan"1  :a  =  R(oh™> 

\  -s  /  \a-?wn/  L(henry) 


:Wo  =  Wn^-?2)W  ;W„"=  — 

M 


2  _  K 


= 


B 
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What  can  be  stated  at  this  point  is  that  there  appears  to  be  no  significant  contribution  to  the 
control  loop  structure  from  the  electro-hydraulic  valves  for  times  greater  than  .14  s.  Therefore, 
the  servo  valves  section  is  now  clearly  defined  by  the  equation  for  ram  velocity. 

The  next  two  blocks  consisting  of  the  link-movers  and  hydraulic  system  are  machine  depen¬ 
dent  and  the  subject  of  considerable  modeling.  Therefore,  this  section  will  be  presented  in 
detail  during  October  of  FY84. 

Using  the  program  in  Appendix  C,  the  ram  velocity  Xr(t)  is  plotted  as  a  function  of  time  with 
the  result  shown  in  Figure  14.  The  significance  is  that  the  solution  of  this  third  order  system 
has  resulted  in  a  fairly  well  damped  curve  with  no  overshoot  of  the  steady  state  valve. 

Including  the  optical  encoder  .ection,  essentially  a  time  delay  only,  the  control  loop  structure 
has  been  currently  determined  to  be  as  shown  in  Figure  15. 

VIII.  EXPERIMENTAL  RESULTS 

This  section  is  included  for  completeness  only  since  evaluation  of  the  system  is  still  in  pro¬ 
gress.  The  preliminary  results  from  several  software  sequence  tests  indicate  that  data  dropouts 
and  the  necessary  error  checking  are  adding  an  additional  2  to  4  s  to  the  time  needed  for  the 
state  driving  program  to  update  its  driving  voltages.  Therefore,  the  desired  final  position  is  ex¬ 
ceeded.  Several  alternatives  have  been  proposed  to  the  use  of  the  6522  VIA  shift  registers.  Cur¬ 
rently,  a  switching  from  serial  to  parallel  intercommunication  between  the  two  AIM  65s  is  in 
progress. 

IX.  CONCLUSIONS 

The  principle  findings  of  this  review  are  as  follows: 

Currently  the  state  determinations,  state  sensing,  and  data  transfer  (96-bit  stream)  programs 
have  been  designed,  developed,  and  tested  on  the  prototype  demonstrator.  All  hardware 
modifications  have  been  completed. 

In  the  future,  it  is  expected  that  a  complete  open  loop/closed  loop  servo  response  with  a  full 
system  will  be  analyzed  to  determine  deviations  from  expected  valve  due  to  the  mechanical 
uncertainities  in  the  system  and  the  maximum  drive  rate  of  the  hydraulic  system. 


Additionally,  it  is  anticipated  that  considerable  work  needs  to  be  done  i 
main  control  program  and  controller  so  that  considerations  such  as: 

a.  Reference  point  motion, 

b.  Inertial  load  changes, 

c.  Real  time  response,  and 

d.  Multiple  processing 

are  included  in  the  prototype  demonstration. 
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PROGRAM; 

STATE  DETERMINATION; 


(K)*=200 

/99 

0200  A9  LDA  #00 
0202  8D  STA  A003 
0205  A9  LDA  #2C 
0207  80  STA  A002 
020A  A9  LDA  #00 
020C  8D  STA  AOOO 
020F  AO  LDA  A001 
0212  80  STA  0400 
0215  AD  LDA  AOOO 
0218  8D  STA  0401 
021B  A9  LDA  #04 
02 ID  8D  STA  AOOO 
0223  8D  STA  0402 
0226  AD  LDA  AOOO 
0229  8D  STA  0403 
022C  A9  LDA  #08 
022E  8D  STA  AOOO 
0231  AO  LDA  A001 
0234  8D  STA  0404 
0237  AD  LDA  AOOO 
023A  8D  STA  0405 
023D  AD  LDA  #0C 
023F  80  STA  AOOO 
0242  AD  LDA  A001 
0245  8D  STA  0406 
0248  AD  LDA  AOOO 
024B  80  STA  0407 
024E  EA  NOP 


Encoder  Select/Data  Read  Program 
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024F  38  SEC 
0250  AO  LDA  0408 
0253  ED  SBC  0400 
0256  80  STA  0410 
0259  AD  LDA  0409 
025C  ED  SBC  0401 
025F  80  STA  0411 
0262  90  BCC  02C0 
0264  38  SEC 
0265  AD  LDA  040A 
0268  ED  SBC  0402 
026B  8D  STA  0412 
026E  AD  LDA  040B 
0271  ED  SBC  0403 
0274  8D  STA  0413 
0277  90  BCC  02CF 
0279  38  SEC 
027A  AD  LDA  040C 
027D  ED  SBC  0404 
0280  8D  STA  0414 
0283  AD  LDA  040D 
0286  ED  SBC  0405 
0289  8D  STA  0415 
028C  90  BCC  02DF 
028E  38  SEC 
028F  AD  LDA  040E 
0292  ED  SBC  0406 
0295  80  STA  0416 
0298  AD  LDA  040F 
029B  ED  SBC  0407 
029E  8D  STA  0417 
02A1  90  BCC  02EF 
02A3  EA  NOP 
02A4  EA  NOP 
02A5  4C  JMP  0300 
02A8  EA  NOP 
02A9  EA  NOP 
02AA  EA  NOP 
02AB  EA  NOP 


Change  ADJ 
Sign 


No  change 
ADJ.  Sign 


Multiprecision  Routine  for  State  Data 
Completed  Oct  82.  R.O.  Chavez 


29 


02  AC 

EA 

NOP 

02  AD 

EA 

NOP 

02AE 

EA 

NOP 

02  AF 

EA 

NOP 

02B0 

EA 

NOP 

0281 

EA 

NOP 

02B2 

EA 

NOP 

02B3 

EA 

NOP 

02B4 

EA 

NOP 

02B5 

EA 

NOP 

02B6 

EA 

NOP 

02B7 

EA 

NOP 

02B8 

EA 

NOP 

02B9 

EA 

NOP 

02BA 

EA 

NOP 

02BB 

EA 

NOP 

02BC 

EA 

NOP 

02  BD 

EA 

NOP 

02BE 

EA 

NOP 

02BF 

EA 

NOP 

02C0 

AD 

LDA  0411 

02C3 

09 

ORA  #04 

02C5 

8D 

STA  0411 

02C8 

4C 

JMP  0264 

02CB 

EA 

NOP 

02CC 

EA 

NOP 

02CD 

EA 

NOP 

02CE 

EA 

NOP 

02CF 

AD 

LDA  0413 

02D2 

09 

ORA  #04 

02D4 

8D 

STA  0413 

02D7 

4C 

JMP 

0279 

02  DA 

EA 

NOP 

02  DB 

EA 

NOP 

02  DC 

EA 

NOP 

02  DD 

EA 

NOP 

02  DE 

EA 

NOP 

02  DF 

AD 

LDA 

0415 

02E2 

09 

ORA 

#04 

02E4 

8LD  STA  0415 

02E7 

4C 

JMP 

028E 

02EA 

EA 

NOP 

02EB 

EA 

NOP 

02EC 

EA 

NOP 

02ED 

EA 

NOP 

02EE 

EA 

NOP 

02EF 

AD 

LDA 

0417 

02F2 

09 

ORA 

#04 

02F4 

8D 

STA 

0417 

02F7 

4C 

JMP 

0300 

02FA 

EA 

NOP 

02FB 

EA 

NOP 

02FC 

EA 

NOP 

02FD 

EA 

NOP 

02FE 

EA 

NOP 

02FF 

EA 

NOP 

Expansion  Area  &  Sign  Manipulation 
Completed  Dec  82,  R.J.  Chavez 


V 


n 


■r. 

'i 


I 


; 


ft 


>v 


- 


0300  A2 
0302  0E 
0305  OE 
0308  OE 
030B  OE 
030E  E8 
030F  EO 
0311  FO 
0313  4C 
0316  A2 
0318  4E 
031B  4E 
031E  4E 
0321  4E 
0324  E8 
0325  EO 
0327  FO 
0329  4C 
032C  EA 
C32D  EA 
032E  EA 
032F  A9 
0331  80 
0334  AD 
0337  8D 
033A  AD 
033D  8D 
0340  A9 
0342  80 
0345  AD 
0348  8D 
034B  AD 
034E  8D 
0351  A9 
0353  8D 
0356  AD 
0359  8D 
03 5C  AD 
035F  8D 
0362  A9 
0364  8D 
0367  AD 
036A  80 
036D  AD 
0370  80 


LDX  #00 
ASL  0411 
ASL  0413 
ASL  0415 
ASL  0417 


INX 
CPX  #05 


BEQ  0316 
JMP  0302 
LDX  #00 
LSR  0411 
LSR  0413 
LSR  0415 
LSR  0417 


INX 
CPX  #05 


BEQ  032C 
JMP  0318 
NOP 
NOP 
NOP 

LDA  #00 
STA  0600 
LDA  0410 
STA  0601 
LDA  0411 
STA  0602 
LDA  #10 
STA  0603 
LDA  0412 
STA  0604 
LDA  0413 
STA  0605 
LDA  #20 
STA  0606 
LDA  0414 
STA  0607 
LDA  0415 
STA  0608 
LDA  #30 
STA  0609 
LDA  0416 
STA  060A 
LDA  0417 
STA  060B 


C373 
0374 
0376 
0377 
0378 
0379 
037A 
037B 
037C 
037D 
037E 
037F 
0380 
0381 
0382 
0383 
0384 
0385 
0386 
0387 
0388 
0389 
03 8 A 
038B 
038C 
03 8D' 
038E 
038F 


EA  NOP 
EA  NOP 
EA  NOP 
EA  NOP 
EA  NOP 
EA  NOP 
EA  NOP 
EA  NOP 
EA  NOP 
EA  NOP 
EA  NOP 
EA  NOP 
EA  NOP 
EA  NOP 
EA  NOP 
EA  NOP 
EA  NOP 
EA  NOP 
EA  NOP 
EA  NOP 
EA  NOP 
EA  NOP 
EA  NOP 
EA  NOP 
EA  NOP 
EA  NOP 
EA  NOP 
EA  NOP 


Expansion  Area  &  96  Bit  (full)  Data  Stream  Manipulation 
Completed  (1)  Mar  82,  Dec  82,  R.J.  Chavez 
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r.  -* 


^ 


_  W*  -V 


I 


J 


I 

I 


0390 

18 

CLC 

0391 

AO 

LOA 

AOOO 

0394 

4A 

LSR 

A 

0395 

4A 

LSR 

A 

0396 

4A 

LSR 

A 

0397 

4A 

LSR 

A 

0398 

OA 

ASL 

A 

0399 

OA 

ASL 

A 

039A 

OA 

ASL 

A 

03  9B 

OA 

ASL 

A 

039C 

OA 

ASL 

A 

0390 

OA 

ASL 

A 

039E 

OA 

ASL 

A 

039F 

4A 

LSR 

A 

03A0 

4A 

LSR 

A 

03A1 

4A 

LSR 

A 

03A2 

EA 

NOP 

03A3 

EA 

NOP 

03A4 

EA 

NOP 

03A5 

EA 

NOP 

03A6 

EA 

NOP 

03A7 

EA 

NOP 

03A8 

EA 

NOP 

03A9 

C9 

CMP 

#10 

03AB 

FO 

BEQ 

03B0 

03AD 

4C 

JMP 

0200 

03B0 

A9 

LOA 

#20 

03B2 

8D 

STA 

AOOO 

03B5 

20 

JSR 

ED2C 

03B8 

20 

JSR 

ED2C 

03BB 

20 

JSR 

ED2C 

03BE 

20 

JSR 

ED2C 

03C1 

20 

JSR 

ED2C 

03C4 

20 

JSR 

ED2C 

03C7 

EA 

NOP 

03C8 

EA 

NOP 

03C9 

EA 

NOP 

03CA 

EA 

NOP 

03CB 

EA 

NOP 

03CC 

EA 

NOP 

03CD 

EA 

NOP 

03CE 

EA 

NOP 

03CF 

EA 

NOP 

go  to  transmit  routine 
SET  Bit  HI 


Data  Transfer  Handshake  Routine  Completed 
Single:  April  82,  Multi:  (a)  Dec  82,  (b) 
R.J.  Chavez 
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03  DO 

A2 

LDX 

#00 

0302 

AO 

LDY 

#00 

03D4 

A9 

LDA 

#00 

03D6 

8D 

STA 

AOOB 

03D9 

A9 

LDA 

#14 

03DB 

8D 

STA 

AOOB 

03  DE 

A9 

LDA 

#20 

03E0 

80 

STA 

A008 

03E3 

A9 

LDA 

#00 

03E5 

8D 

STA 

A009 

03E8 

BD 

LDA 

0600, X 

03E8 

8D 

STA 

AOOA 

03EE 

C8 

INY 

03EF 

CO 

CPY 

#FF 

03F1 

DO 

BN  E 

03EE 

03F3 

E8 

INX 

03F4 

EO 

CPX 

#0C 

03F6 

DO 

BNE 

03D2 

03F8 

A9 

LDA 

#00 

03FA 

8D 

STA 

AOOO 

03FD 

00 

BRK 

Transmit 
96  bit 
stream 


Data  Transfer  Routine  Completed 
Single:  (a)  April  82,  Multi:  (a)  Dec  82  (15),  (b) 

R.J.  Chavez 


PROGRAM  CONTROL; 

USES  TRANSCEND; 

CONST  A2MAX=75;A4MIN=0;MHL=16;MHW=16;MHD=6; (*MAX  HOLE  DIM'S*) 
80LE=8. 4;DILE=6. 1;BULE=3 . 2; BUW 1=1 . 5 ; BUDE=1 .5; 

TYPE  SPEC=ARRAY[1 . .MHL, 1 . .MHW]  OF  INTEGER; 

VAR  CM , D IMODE , DUMODE : CHAR ; 

HOSPEC,SPSPEC,HOSTATE,SPSTATE :SPEC; 
Al,A2,A3,A4,AIl,AI2,AI3,AI4, 

Dl, 02,03, D4, I, J,K,L,W,D,P0T1,P0T2,P0T3,P0T4,: INTEGER; 

BL ,  BW ,  BD ,  X ,  Y ,  Z :  RE  AL ; 

SEGMENT  PROCEDURE  HOLESPEC; 

(♦CREATES  SPEC*) 

VAR  P: INTEGER; 

BEGIN 

WRITELNC  INPUT  HOLE  LENGTH,  INTEGERS  ,MHL) ; 

READLN(L); 

P:=1+TRUNC(L/BULE) ; 

BL  :=L/P; 

L  :=P* 

WRITENLNC  INPUT  HOLE  WIDTH,  EVEN  INTEGER < 1  ,MHW) ; 

READLN(W) ; 

P :=1+TRUNC(W/BUWI ) ; 

8W:=W/P; 

W  I  =P  9 

WRITELNC  INPUT  HOLE  DEPTH,  INTEGER< '  ,MHD; 

REAOLN(D); 

P:=1+TRUNC(D/BUDE) ; 

BD:=D/P; 

D:=P; 

FOR  I:=l  TO  MHL  DO 
FOR  J:=l  TO  MHW  DO 

IF  (JXMHW/2-W/2))  AND  ( J<(MHW/2+W/2) )  AND  (I<L) 

AND ( SQRT ( SQR ( I*BL )+SQR ( J*BW)+SQR (K*BD) ) <BOLE+D ILE+BULE ) 
THEN  HOSPECCI , J] :=D 
ELSE  HOSPECCI, J]:=0; 

END; 

SEGMENT  PROCEDURE  SPOILSPEC; 

(♦CREATES  SPEC*) 

VAR  C:CHAR; 

P,L,W: INTEGER ; 

BEGIN 

WRITELN( ’ SPOIL  TO  R/IGHT,  L/EFT  OR  B/OTH  SIDES?'); 

READ(C) ; 

WRITELNC  INPUT  LENGTH,  INTEGERS  ,MHL) ; 

READLN(L); 

P:=1+TRUNC(L/BULE); 

BL :L/P; 

L  :=P; 

WRITELNC  INPUT  WIDTH,  INTEGER < '  ,MHW) ; 


V  V 


i 


•x 


READLN(W) ; 

P : =1+TRUNC (W/BUW I ) ; 

BW:=W/P; 

W:=P' 

FOR  I :  =1  TO  MHL  DO 
FOR  J:=l  TO  MHW  DO 
IF  I<L  THEN 
CASE  C  OF 

* R ' : IF  (HOSPECCI ,J]=0)  AND  (J>MHW/2)  AND  ( J<(MHW-W)/2)  AND 
(SQRT( SQR ( I*BL)+SQR ( J*BW)+SQR (K*BD) ) <BOLE+D ILE+BULE ) 

THEN  SPSPEC[I,J]:=1  ELSE  SPSPECCI , J]:=0; 


► 

L*  • 


* 


' L ' :  IF  (HOSPECCI, J]=0)  AND  (J<MHW/2)  AND  ( J>(MHW-W)/2)  AND 
(SQRT(SQR( I*BL)+SQR( J*BW)+SQR(K*BD) )<BOLE+D ILE+BULE) 

THEN  SPSPECCI, J]:»l  ELSE  SPSPECCI, J]:=0; 

’ B ’ : IF  HOSPECCI, J]=0 

(SQRT ( SQR ( I*BL)+SQR( J*8W)+SQR(K*BD) ) <BQLE+DILE+8ULE) 

THEN  SPSPECCI, J]:=0; 

END 

ELSE  SPSPECCI, J]:=0; 

END; 

PROCEDURE  QUIT;FORWARD; 

PROCEDURE  ATDMP; FORWARD; 

PROCEDURE  ATOD EXTERNAL; 

PROCEDURE  GETPOS; EXTERNAL; 

PROCEDURE  SENDPOS; EXTERNAL; 

SEGMENT  PROCEDURE  MANUAL; 

(♦PROVIDES  JOYSTICK  CONTROL*) 

CONST  K-l; 

VAR  C:CHAR; 

PROCEDURE  TEMPLATE; 

BEGIN 

WRITELNC PROCEDURE  TEMPLATE'); 

IF  (Z<0)  AND  ( ( X >L )  OR  (Y>W/2)  OR  (Y<-W/2)  OR  (Z<D) )  THEN 

BEGIN 

Al:*AIl; 

A2:=AI2; 

A3:*AI3; 

A4:=AI4; 

END; 

END; 


36 


PROCEDURE  LOCATE; 

VAR  RPrREAL; 

BEGIN  , 

Z:=B0LE*S1N(A2)+DILE*SIN(A2+A3)+BULE*SIN(A2+A3+A4) ; 
RP:=B0LE*C0S(A2)+DILE*C0S(A2+A3)+BULE*C0S(A2+A3+A4) 
X:=RP*COS(Al) ; 

Y:=RP*SIN(A1) ; 

END;  j 

BEGIN 

REPEAT 

BEGIN 

GETPOS; 

ATOD; 

Al:=AIl+K*POTl; 

A2:=AI2+K*POT2; 

A3:=AI3+K*POT3; 

A4:=AI4+K*P0T4; 

IF  DIMODE='A'  THEN 
BEGIN 
LOCATE; 

TEMPLATE; 

END; 

SENDPOS; 

(*IF  DUMODE= 1  A'  THEN  CHECKCURL; 

IF  CURLED  THEN  ATDUMP;*) 

END; 

UNTIL  C3 'Q' ; 

END; 

PROCEDURE  DIG; 

BEGIN 

END; 

PROCEDURE  WAIT; 

BEGIN 

REPEAT 

GETPOS; 

UNTIL  (AI1=A1)  AND  (AI2=A2)  AND  (AI3-A3); 

END; 

PROCEDURE  SWING; 

BEGIN 

Al: =TRUNC(ATAN(X/Y) ) ; 

WAIT; 

SENDPOS; 

END; 
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PROCEDURE  REACH; 

VAR  S,VEAN,VELE:REAL; 
BEGIN 


VELE:=SQRT(SQR(Y)+SQR(Z) ) ; 

VEAN:=ATAN(Z/Y) ; 

S:=(BOLE+DILE+VELE)/2; 

A2 : =  TRUNC ( 2*ATAN ( SQRT ( ( S-VELE ) * ( S-BOLE ) /S* (S-DILE)) )-VEAN) 

A3:sTRUNC(2*ATAN(SQRT( (S-BOL£)*(S-DILE)/S*(S-VELE)) ) ) ; 

WA  IT; 

SENOPOS; 

END; 


PROCEDURE  QUIT; 

BEGIN 

WRITELN( 'PROCEDURE  QUIT'); 
ATDMP ; 

(♦CENTER  BOOM 
HOME 

PROMPT  OPERATOR  TO  SECURE*) 
END; 


PROCEDURE  SWEEP; 

BEGIN 

END; 

PROCEDURE  ATDMP; 

BEGIN 

WRITELNCP  ATDMP'); 

FOR  I:*MHL  DOWN  TO  1  DO 
BEGIN 

IF  SPSTATEC I ,MHH]-SPSTATE[ 1+1,13=1  THEN 
BEGIN 
DUMP; 

SPSTATE[ I ,0] :=SPSTATE[ I , J]+l; 

END; 


PROCEDURE  AUTO; 

VAR  S:REAL; 

BEGIN 

WRITELN( 'PROCEDURE  AUTO'); 

FOR  K:=l  TO  D  DO 

BEGIN 

FOR  I :=1  TO  MHL  DO 
FOR  J : =1  TO  MHW  DO 
BEGIN 

IF  HOSPECCI, J]<>0  THEN 
8EGIN 

A2:=A2MAX; 

A3:=90-A2; 

A4:=A4MIN; 

SENDPOS; 


Z:=8D*(1-HGSTATE[ I, J3) ; 

Y :=BL*J; 

X  :=BW*( 1-8) ; 

SWING; 

REACH; 

DIG; 

HOSTATEC I , J] : =HOSTATE[ I ,  J  3+1 ; 

ATDMP ; 

END; 

END; 

SWEEP; 

END; 

END; 

BEGIN  (*MAIN*) 

PAGE(OUTPUT); 

WR ITELN ( 'E/XCAVATE  OR  Q/UIT? 1 ) ; 

READ (CM); 

IF  CM= 1 Q 1  THEN  QUIT  ELSE 
BEGIN 

WR ITELN ('M/ANUAL,T/EMPLATE,  OR  A/UTO  MODE?'); 

READ(CM) ; 

CASE  CM  OF 
' M ' : BEGIN 

DIMODE:='M' ; 

DUMODE:='M' ; 

MANUAL; 

END; 

'T* :  BEGIN 

WR  ITELN  ('M/ANUAL  OR  S/PECIFIED  SPOIL  DEPOSITION?') 
READ(CM) ; 

IF  CM= ' S 1  THEN 
BEGIN 

DUMODE : = ' A ' ; 

HOLESPEC; 

SPOILSPEC; 

END 

ELSE  DUMODE:='M' ; 

MANUAL; 

END; 


'A':  BEGIN 

HOLESPEC; 

SPOILSPEC; 


-PROC  SENDPOS 


.PUBLIC 

RETURN  .EQU 
PLA 
STA 
PLA 
STA 
LDA 
STA 
LDA 
STA 
LDA 
PHA 
JSR 
LDA 
PHA 
JSR 
LDA 
PHA 
JSR 
LOA 
PHA 
JSR 
LOA 
PHA 
JSR 
LDA 
PHA 
JSR 
LDA 
PHA 
JSR 
LDA 
PHA 
JSR 
LDA 
PHA 
LDA 
PHA 
RTS 

OUTPUT  LDS 
AND 
BNE 
PLA 
STA 
RTS 

.END 


A1.A2.A3.A4 

0 

RETURN 

RETURN+1 

#03  ;RESET  ACIA 

OCOAO 

11  ;COM  REG. 

OCOAO 

Al 

OUTPUT 

Al+1 

OUTPUT 

A2 

OUTPUT 

A2+1 

OUTPUT 

A3 

OUTPUT 

A3+1 

OUTPUT 

A4 

OUTPUT 

A4+1 

OUTPUT 

RETURN+1 

RETURN 


OCOAO 

#2 

OUTPUT 

0C0A1 


40 


av. 


*  .  •  .  »  -  ■ 


!■:: 


I 


,v- 


RETURN 


.MACRO  POP 
PLA 
STA  XI 
PLA 

STA  Xl+1 
.ENDM 


•MACRO  WAIT 

NOP 

NOP 

NOP 

NOP 

NOP 

NOP 

.ENOM 


.PROC  ATOO 

.PUBLIC  P0T1,P0T2,P0T3,P0T4 


.EQU 

POP 

LDA 

WAIT 

LDA 

STA 

LDA 

PHA 

LDA 

PHA 

RTS 


0 

RETURN 

OCOCO 


OCOCO 

POTl 

RETURN+1 


RETURN 


.END 


LIST 

68  REM*** ******************** ********** 
70  REM*****PROGRAM  VALVE  RESPONSE****** 

72  REM*****  6MAY83  ****** 

74  REM 

76  REM*****  BY:  R.  CHAVEZ  ***** 

78  REM*****  C.  CLARK  ***** 

80  REM*  * 

82  REM********************************* 
100  I N I T 
1 10  PACE 

120  A$  =  "RES  I  STANCE (OHM  1  = 

130  B*= " MAGNET i C  F I  EL 0 ( DYNE /CM/ SEC )  =  " 
140  C$="SPRINC" 

150  V$="COEFFICIENT (DYNE/CM) =" 

160  D$= "MASS ( GRAM ) = 

170  E*= " I NOUCTANCE ( HE NR  I ES 1 = " 

180  Fs=  "  AMP  FACTOR  (VOLT/VOLT  1  ='* 

190  PRINT  A* 

200  INPUT  A 1 
210  PRINT  B$ 

220  INPUT  B I 
230  PRINT  C$ 

240  PRINT  V* 

250  INPUT  Cl 
260  PRINT  D$ 

270  INPUT  01 
280  PRINT  E$ 

290  INPUT  El 
300  PRINT  F$ 

310  INPUT  FI 
320  PRINT 
330  PRINT  A* , A  1 
340  PRINT  B$ , B 1 
350  PRINT  C$ 

360  PRINT  V$,C1 
370  PRINT  D*,D1 
380  PRINT  E* , E 1 


44 
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390  PRINT  F*,F1 

400  P$= "ENTRY  VALID  =1  N0T=2" 

410  PRINT 
420  PRINT  P* 

430  INPUT  PI 
440  IF  P I < 1  THEN  100 
450  IF  P 1 >2  THEN  100 
460  IF  PI =2  THEN  100 

470  REM  NOTE  THAT  PROGRAM  REQUIRES  RADIANS 
480  DIM  X M 000) , A U 000) 

490  REMVIEWPORT  50.130,50.100 

500  A  2  =  A  1 / E  1 

510  C2  =  C  » /D 1 

520  C3=C2T0  5 

530  B2=B 1 / l 2* ( C I *D 1 )T0.5) 

540  B3=82T2 
550  B4  = ( 1 -93) T0.5 
560  C4=C3*B4 
570  R 1 =B4/ (B2*- 1 ) 

580  R2=C4/ (A2-B2*C3) 

590  R3=- 1 YATN 1R1) -ATN (R2) 

600  R4=C2/ ( A2T2-2*B2*C3*A2+C2) 

610  R5  =  A2/ (B4*  f A2T2-2*B2*C3*A2+C2) T0  5) 

620  MOVE  40.45 

630  SCALE  140/60.280/45 

640  PAGE 

650  AXIS  5. 10 

660  FOR  1=1  TO  140 

670  V=  I  / 1 000 

680  X ( I ) =200* ( I -R4*EXP ( -A2*V) +R5*EXP ( -B2*C3*V ) *S I N ( C4*V-R3 ) ) 

690  REMA ( I ) =200*  f 1 -EXP ( -  1 20*V ) +0 . 4 1 *EXPl-60*V)*SIN( 294 *V- 3 .  14)) 

700  REM  2.49=143/57.3  AND  3.14=180/57.3 

710  NEXT  I 

720  MOVE  1  . X  f 1  ) 

730  FOR  1=1  TO  140 
740  DRAW  I . X  (I) 

750  NEXT  I 
760  MOVE  0.0 


770  REMFOR  1=1  TO  140 
780  REMDRAW  I  . A  (  I ) 

790  REMNEXT  I 

800  HOME 

810  MOVE  50,-40 

820  SS  =  "TIME fsec)  .0001  to .  14" 

830  PRINT  SS 
840  HOME 

850  MOVE  -50, 180 
860  TS=”Rcm  Velocity” 

870  u*=" (cm/sec)  0»o280" 

880  PRINT  TS 
890  HOME 

900  MOVE  -50, 160 
910  PRINT  US 
920  REMCOPV 
930  REMPAGE 
940  HOME 
942  MOVE  20,-60 

944  PRINT  "*********#*#**  I******************************** ” 

950  MOVE  20,-70 

960  PRINT  AS , A  1 

970  MOVE  20,-90 

980  PRINT  BS , B 1 

990  MOVE  20,-100 

1000  PRINT  CS 

1010  MOVE  20,-110 

1020  PRINT  VS, Cl 

1030  MOVE  20,-120 

1040  PRINT  DS,D! 

1050  MOVE  20.-130 
1060  PRINT  ES.E1 
1070  MOVE  20,-140 
1080  PRINT  FS.Fl 
1090  MOVE  20,-150 
1 100  PRINT 
1110  MOVE  20,-160 
1  120  PRINT  "ALPHAS  " , A2 
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I  I  30  MOVE  20,-170 

1  1 40  PRINT  "  FREO= " , C3 

1  1 50  HOVE  20.-180 

1  160  PRINT  “  FREQ ( 0  5  =  " ,C4 

1  1 70  HOVE  20.-190 

1180  PRINT  "PSI  =  "  ,  R3 

1  1 90  HOVE  20, -200 

1200  PRINT  "COEFFICIENT  l=".R4 

1210  DOVE  20.-210 

1220  PRINT  "COEFFICIENT  2=".R5 

1230  MOVE  20,-220 

1240  PRINT  "¥¥¥¥¥¥¥¥¥*¥¥¥¥¥¥¥*¥¥¥¥¥¥¥¥¥¥¥¥¥¥¥¥¥¥¥¥¥¥*¥¥¥¥¥ 

1  250  MOVE  20 , -240 

1260  PRINT  "MAX  VALUE  X(T) 

!  270  MOVE  20, -250 

1280  PRINT  "VELOCITY  ( CM/SE C 1 - " , X i 1 4 0 ) 

1290  REMCOPY 
I  300  HOME 
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